function [thetaf, thetab, ln] = kin_inverse(x, y, z, phi)
% Inverse Kinematics for Prostate-Bot
% Units are in mm

% Y = height
% Z = length
l = 9; % length of leg
hs = 6; % height of slider
w = 20+3.5+2; % length of platform

hf = y - (w^2 * tan(phi))/(z - w);
hb = y - w * tan(phi) * ((2*w-z)/(z-w));
ln = (z - w) * sqrt(1 + tan(phi)^2);
thetaf = acosd((hf-hs)/l); % arccos in degrees
thetab = acosd((hb-hs)/l); % arccos in degrees

%if imag(thetaf) ~= 0 || imag(thetab) ~= 0
%    sprintf('Error, invalid coordinates\n')
%end

if imag(thetaf) ~= 0 || imag(thetab) ~= 0
    thetaf = 0;
    thetab = 0;
    ln = 0;
end
